#include "stm32f10x.h"
#include "hmc5883.h"
#include "math.h"
#include "iic.h"

#define FILL_NUM 10

int16_t X_HMC, Y_HMC, Z_HMC, x, y, z;
int16_t x_offest = 10, y_offest = 175;
double y_gain = 0.967;

int16_t X_BUFF[FILL_NUM], Y_BUFF[FILL_NUM], Z_BUFF[FILL_NUM];

u8 Init_HMC5883L(void)
{
    u8 date;
    date = IIC_ReadByteFromSlave(HMC5883L_ADDRESS, 0x02, 0x00);
    return date;
}

//******************************************************
// 连续读出HMC5883内部角度数据，地址范围0x3~0x5
//******************************************************
void Multiple_Read_HMC5883L(void)
{
    u8 i;
    uint8_t BUF[8];
    static uint8_t filter_cnt = 0;
    int32_t temp1 = 0, temp2 = 0, temp3 = 0;

    IIC_Start();
    IIC_SendByte(HMC5883L_ADDRESS);
    IIC_WaitAck();
    IIC_SendByte(0x03);
    IIC_WaitAck();

    IIC_Start();
    IIC_SendByte(HMC5883L_ADDRESS + 1);
    IIC_WaitAck();

    for (i = 0; i < 6; i++)
    {
        //BUF[i] = IIC_ReadByte(); // BUF[0]存储数据
        if (i == 5) { IIC_NAck();} 
        else {IIC_Ack();}
    }
    IIC_Stop();
    IIC_Ack();

    X_BUFF[filter_cnt] = BUF[0] << 8 | BUF[1]; // Combine MSB and LSB of X Data output register;
    Y_BUFF[filter_cnt] = BUF[4] << 8 | BUF[5]; // Combine MSB and LSB of Y Data output register;
    Z_BUFF[filter_cnt] = BUF[2] << 8 | BUF[3]; // Combine MSB and LSB of Z Data output register;
    for (i = 0; i < FILL_NUM; i++)             // 10深度的滑动滤波
    {
        temp1 += X_BUFF[i];
        temp2 += Y_BUFF[i];
        temp3 += Z_BUFF[i];
    }
    x = temp1 / FILL_NUM;
    y = temp2 / FILL_NUM;
    z = temp3 / FILL_NUM;

    filter_cnt++;
    if (filter_cnt == FILL_NUM) filter_cnt = 0;

    /*   地磁椭圆矫正                              */
    /*  关于如何进行椭圆矫正 ，将在下版本 详细说明 */
    X_HMC = 1 * (x + x_offest);
    Y_HMC = (double)(y_gain * (y + y_offest));
    Z_HMC = (double)(1.073 * (z + 30));
}